﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;

namespace PistonWheel
{
    class HamiltonianSolver
    {
        public double PistonMass
        {
            get
            {
                return this.Piston.Mass;
            }
            set
            {
                this.Piston.Mass = value;
            }
        }

        public Piston Piston { get; set; }
        public Wheel Wheel { get; set; }
        public Rod Rod { get; set; }
        public double Step { get; set; }

        public readonly double G;

        public HamiltonianSolver(double step, Piston piston, Wheel wheel, Rod rod)
        {
            this.G = -9.81;
            this.Piston = piston;
            this.Wheel = wheel;
            this.Rod = rod;
            this.Step = step;
        }

        /// <summary>
        /// Computes the moment of inertia of rotating wheel
        /// </summary>
        /// <returns></returns>
        public double getI1()
        {
            return 0.5 * this.Wheel.Mass * Math.Pow( this.Wheel.Radius, 2 );
        }

        /// <summary>
        /// Computes the moment of inertia of rod
        /// </summary>
        /// <param name="phi">Wheel angle</param>
        /// <returns>Inertia</returns>
        public double getI2(double phi, double time)
        {
            int steps = 100;
            double rodLength = getRodLength( time );
            double result = 0.0;
            double mi = this.Rod.Mass / (double)steps;
            double psi = Math.Asin( this.Wheel.Radius * Math.Sin( phi ) / getRodLength( time ) );
            for (int i = 0; i < steps; ++i)
            {
                double length = (i + 1) * rodLength / steps;
                double x = length * Math.Cos( psi );
                double y = length * Math.Sin( psi );
                result += mi * (Math.Pow( x, 2 ) + Math.Pow( y, 2 ));
            }
            return result;
        }

        private double getRodLength(double time)
        {
            double meanValue = 0.0;
            double exponent = -Math.Pow( time - meanValue, 2 ) / (2 * Math.Pow( this.Rod.LengthStdDev, 2 ));
            return this.Rod.L + 1.0 / (this.Rod.LengthStdDev * Math.Sqrt( 2 * Math.PI )) * Math.Pow( Math.E, exponent );
        }

        public double getPistonPosition(double phi, double time)
        {
            double ll = Math.Pow( getRodLength( time ), 2.0 ) - Math.Pow( this.Wheel.Radius * Math.Sin( phi ), 2 );
            return Math.Sqrt( ll ) + this.Wheel.Radius * Math.Cos( phi ) + this.Wheel.Middle.Y;
        }

        private double getf(double phi, double t)
        {
            double result = 0.0;
            double pm = this.PistonMass;
            double wr = this.Wheel.Radius;
            double k = -G * this.PistonMass * this.Wheel.Radius;
            double ll = Math.Pow( this.getRodLength( t ), 2.0 ) - Math.Pow( this.Wheel.Radius * Math.Sin( phi ), 2 );

            result += k * Math.Sin( phi );
            result -= this.Wheel.Radius * Math.Sin( phi );
            result -= (Math.Pow( this.Wheel.Radius, 2 ) * Math.Cos( phi ) * Math.Sin( phi )) / Math.Sqrt( ll );
            result += pm * Math.Pow( wr, 2 ) * Math.Cos( phi ) * Math.Sin( phi );
            result += pm / 2.0 * (2 * Math.Pow( wr, 4 ) * Math.Pow( Math.Cos( phi ), 2 ) * Math.Sin( phi )) / ll;
            result -= pm / 2.0 * (2 * Math.Pow( wr, 4 ) * Math.Cos( phi ) * Math.Pow( Math.Sin( phi ), 2 )) / ll;
            result += pm / 2.0 * (2 * Math.Pow( wr, 5 ) * Math.Pow( Math.Cos( phi ), 2 ) * Math.Pow( Math.Sin( phi ), 3 )) / Math.Pow( ll, 1.5 );
            result += pm / 2.0 * (4 * Math.Pow( wr, 3 ) * Math.Pow( Math.Cos( phi ), 2 ) * Math.Sin( phi )) / Math.Sqrt( ll );
            result -= pm / 2.0 * (2 * Math.Pow( wr, 3 ) * Math.Pow( Math.Sin( phi ), 3 )) / Math.Sqrt( ll );
            result /= (this.getI1() + this.getI2( phi, t ));
            return result;
        }

        private double getfAngle(double angVelocity, double time)
        {
            return angVelocity / this.Wheel.Radius;
        }

        public void solve(ref double time)
        {
            //solve angular velocity
            double k1 = getf( this.Wheel.Angle, time );
            double k2 = getf( this.Wheel.Angle + this.Step * k1 / 2.0, time + this.Step / 2.0 );
            double k3 = getf( this.Wheel.Angle + this.Step * k2 / 2.0, time + this.Step / 2.0 );
            double k4 = getf( this.Wheel.Angle + this.Step * k3, time + this.Step );
            double k = this.Wheel.AngularVelocity + this.Step * (k1 + 2.0 * k2 + 2.0 * k3 + k4) / 6.0;
            this.Wheel.AngularVelocity = k;

            //solve angle
            double a1 = getfAngle( this.Wheel.AngularVelocity, time );
            double a2 = getfAngle( this.Wheel.AngularVelocity + this.Step * a1 / 2.0, time + this.Step / 2.0 );
            double a3 = getfAngle( this.Wheel.AngularVelocity + this.Step * a2 / 2.0, time + this.Step / 2.0 );
            double a4 = getfAngle( this.Wheel.AngularVelocity + this.Step * k3, time + this.Step );
            double a = this.Wheel.Angle + (a1 + 2.0 * a2 + 2.0 * a3 + a4) / 6.0;
            this.Wheel.Angle = a;

            time += Step;
        }
    }
}
